#!/usr/bin/env python
# coding:UTF-8
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Vector3
import serial

buf_length = 11

RxBuff = [0] * buf_length
AngleData = [0.0] * 8
CheckSum = 0

start = 0  # Frame start flag
data_length = 0  # Data length based on the protocol

Angle = [0] * 3  # Use integer for angle

def GetDataDeal(list_buf):
    global Angle

    if list_buf[buf_length - 1] != CheckSum:  # Checksum error
        return
        
    if list_buf[1] == 0x53:  # Angle output
        for i in range(6):
            AngleData[i] = list_buf[2 + i]  # Assign valid data
        Angle = get_angle(AngleData)
        
        # Publish angle data
        publish_angle(Angle)

def DueData(inputdata):  # Process incoming data
    global start
    global CheckSum
    global data_length
    
    if inputdata == 0x55 and start == 0:
        start = 1
        data_length = buf_length
        CheckSum = 0
        # Clear buffer
        for i in range(buf_length):
            RxBuff[i] = 0

    if start == 1:
        CheckSum += inputdata  # Checksum calculation includes the checksum byte
        RxBuff[buf_length - data_length] = inputdata  # Save data
        data_length -= 1  # Decrease length
        if data_length == 0:  # Complete data received
            CheckSum = (CheckSum - inputdata) & 0xff
            start = 0  # Reset
            GetDataDeal(RxBuff)  # Process data

def get_angle(datahex):
    rxl = datahex[0]
    rxh = datahex[1]
    ryl = datahex[2]
    ryh = datahex[3]
    rzl = datahex[4]
    rzh = datahex[5]
    k_angle = 180.0
    angle_x = int((rxh << 8 | rxl) / 32768.0 * k_angle)
    angle_y = int((ryh << 8 | ryl) / 32768.0 * k_angle)
    angle_z = int((rzh << 8 | rzl) / 32768.0 * k_angle)
    if angle_x >= k_angle:
        angle_x -= 2 * k_angle
    if angle_y >= k_angle:
        angle_y -= 2 * k_angle
    if angle_z >= k_angle:
        angle_z -= 2 * k_angle
    return angle_x, angle_y, angle_z

def publish_angle(angle):
    global pub
    angle_msg = Vector3()
    angle_msg.x = angle[0]  # roll
    angle_msg.y = angle[1]  # pitch
    angle_msg.z = angle[2]  # yaw
    pub.publish(angle_msg)

def imu_node():
    global pub
    
    rospy.init_node('imu_node', anonymous=True)
    pub = rospy.Publisher('imu_angle_cmd', Vector3, queue_size=10)
    
    port = rospy.get_param('~port', '/dev/ttyUSB0')
    baud = rospy.get_param('~baud', 115200)
    
    ser = serial.Serial(port, baud, timeout=0.5)
    rospy.loginfo("Serial is Opened: %s", ser.is_open)
    
    rate = rospy.Rate(10)  # 10 Hz
    while not rospy.is_shutdown():
        if ser.in_waiting > 0:
            RXdata = ser.read(1)  # Read one byte at a time
            RXdata = ord(RXdata) #int(RXdata.hex(), 16)  # Convert to hex
            DueData(RXdata)
        
        #rate.sleep()

if __name__ == '__main__':
    try:
        imu_node()
    except rospy.ROSInterruptException:
        pass
